#include "CarMonitor.h"
#include "CarMsgHandle.h"


carMonitorWin::carMonitorWin(QWidget* parent,carSubscriber& sub,ros::NodeHandle _nh)
    : QMainWindow(parent)
    , ui(new Ui_CarMonitor),carSub(sub)//将小车订阅者传进来，用于获取小车数据
{
    ui->setupUi(this);
    pub=_nh.advertise<carMonitor::motion_state>("/car_control",1);
    //方向键槽函数连接
    connect(ui->up_pushButton, &QPushButton::pressed, this, &carMonitorWin::button_slot);
    connect(ui->down_pushButton, &QPushButton::pressed, this, &carMonitorWin::button_slot);
    connect(ui->left_pushButton, &QPushButton::pressed, this, &carMonitorWin::button_slot);
    connect(ui->right_pushButton, &QPushButton::pressed, this, &carMonitorWin::button_slot);
    //方向显示槽函数连接
    //connect(this, SIGNAL(directSignal(QString)), this, SLOT(directCallback(QString)));
    connect(this,&carMonitorWin::directSignal,this,&carMonitorWin::direct_slot);
    //方向键按下状态槽函数连接
    // connect(ui->up_pushButton,&QPushButton::pressed,this,&carMonitorWin::buttonPressed_Bg_slot);
    // connect(ui->down_pushButton,&QPushButton::pressed,this,&carMonitorWin::buttonPressed_Bg_slot);
    // connect(ui->left_pushButton,&QPushButton::pressed,this,&carMonitorWin::buttonPressed_Bg_slot);
    // connect(ui->right_pushButton,&QPushButton::pressed,this,&carMonitorWin::buttonPressed_Bg_slot);
    //connect(ui->down_pushButton,&QPushButton::released,this,&carMonitorWin::buttonReleased_Bg_slot);
    //使用定时器恢复按键颜色
    connect(&timer,&QTimer::timeout,this,&carMonitorWin::buttonReleased_Bg_slot);
    timer.setSingleShot(true);//只触发一次
    motionTimer.setSingleShot(true);//只触发一次
    connect(&motionTimer,&QTimer::timeout,this,&carMonitorWin::motionTimer_slot);
}

void carMonitorWin::display(){
    while(ros::ok()){
        carInfo=carSub.getCarInfo();//获取小车数据
        if(carInfo.check==false){continue;}//表示没读到数据，或者数据没有存储完
        float linear=carInfo.linear;//线速度
        ui->car_linear_lable->setText(QString::number(linear, 'f', 2)+" m/s");
        float voltage=carInfo.voltage;//电压
        ui->car_voltage_lable->setText(QString::number(voltage, 'f', 2)+" V");
        QString light="关";//灯光
        if(carInfo.light==2){light="开";}
        ui->car_light_lable->setText(light);
        //motor1
        ui->current1_lable->setText(name_current+QString::number(carInfo.motor1.current, 'f', 2));
        ui->rpm1_lable->setText(name_rpm+QString::number(carInfo.motor1.rpm, 'f', 2));
        ui->voltage1_lable->setText(name_driverVoltage+QString::number(carInfo.motor1.voltage, 'f', 2));
        ui->temp1_lable->setText(name_temper+QString::number(carInfo.motor1.temper, 'f', 2));
        ui->driverTemp1_lable->setText(name_driverTemper+QString::number(carInfo.motor1.driverTemper, 'f', 2));
        //motor2
        ui->current2_lable->setText(name_current+QString::number(carInfo.motor2.current, 'f', 2));
        ui->rpm2_lable->setText(name_rpm+QString::number(carInfo.motor2.rpm, 'f', 2));
        ui->voltage2_lable->setText(name_driverVoltage+QString::number(carInfo.motor2.voltage, 'f', 2));
        ui->temp2_lable->setText(name_temper+QString::number(carInfo.motor2.temper, 'f', 2));
        ui->driverTemp2_lable->setText(name_driverTemper+QString::number(carInfo.motor2.driverTemper, 'f', 2));
        //motor3
        ui->current3_lable->setText(name_current+QString::number(carInfo.motor3.current, 'f', 2));
        ui->rpm3_lable->setText(name_rpm+QString::number(carInfo.motor3.rpm, 'f', 2));
        ui->voltage3_lable->setText(name_driverVoltage+QString::number(carInfo.motor3.voltage, 'f', 2));
        ui->temp3_lable->setText(name_temper+QString::number(carInfo.motor3.temper, 'f', 2));
        ui->driverTemp3_lable->setText(name_driverTemper+QString::number(carInfo.motor3.driverTemper, 'f', 2));
        //motor4
        ui->current4_lable->setText(name_current+QString::number(carInfo.motor4.current, 'f', 2));
        ui->rpm4_lable->setText(name_rpm+QString::number(carInfo.motor4.rpm, 'f', 2));
        ui->voltage4_lable->setText(name_driverVoltage+QString::number(carInfo.motor4.voltage, 'f', 2));
        ui->temp4_lable->setText(name_temper+QString::number(carInfo.motor4.temper, 'f', 2));
        ui->driverTemp4_lable->setText(name_driverTemper+QString::number(carInfo.motor4.driverTemper, 'f', 2));
    
        
        // ROS_INFO("UI update!");
        //  this->update();
        //计算小车行驶方向
        int th_rpm=50;//防止小车停止时，转速出现负数影响结果
        if(carInfo.motor1.rpm< th_rpm && carInfo.motor2.rpm>th_rpm && carInfo.motor3.rpm>th_rpm && carInfo.motor4.rpm<th_rpm){
            carDirect="up";
        }
        else if(carInfo.motor1.rpm>th_rpm && carInfo.motor2.rpm<th_rpm && carInfo.motor3.rpm<th_rpm && carInfo.motor4.rpm>th_rpm){
            carDirect="down";
        }
        else if(carInfo.motor1.rpm>th_rpm && carInfo.motor2.rpm>th_rpm && carInfo.motor3.rpm>th_rpm && carInfo.motor4.rpm>th_rpm){
            carDirect="right";
        }
        else if(carInfo.motor1.rpm<0 && carInfo.motor2.rpm<th_rpm && carInfo.motor3.rpm<th_rpm && carInfo.motor4.rpm<th_rpm){
            carDirect="left";
        }
        else{
            carDirect="static";
        }
        emit directSignal(carDirect);//发送信号
        this_thread::sleep_for(chrono::milliseconds(100));//延时200ms
    }
    ROS_INFO("线程结束！");
    ROS_INFO("节点状态：%d",ros::ok());
}
void carMonitorWin::startMonitor(){
    ROS_INFO("启动检测线程");
    threadObj=std::thread(&carMonitorWin::display,this);

}
//关闭程序事件
void carMonitorWin::closeEvent(QCloseEvent *event){
    
    ros::shutdown();//关闭节点

    this_thread::sleep_for(chrono::milliseconds(500));//延时500ms
    qInfo()<<"程序退出";
    this->close();//关闭程序
}
//键盘输入事件
void carMonitorWin::keyPressEvent(QKeyEvent *event) {
    if(event->key()==Qt::Key_W){carRun("up");}
    else if(event->key()==Qt::Key_S){carRun("down");}
    else if(event->key()==Qt::Key_A){carRun("left");}
    else if(event->key()==Qt::Key_D){carRun("right");}
    QMainWindow::keyPressEvent(event);//调用基类的事件处理函数
}
//键盘松开事件
void carMonitorWin::keyReleaseEvent(QKeyEvent *event) {
     if(event->key()==Qt::Key_W | event->key()==Qt::Key_A|event->key()==Qt::Key_S|event->key()==Qt::Key_D) {
       carStop();
     }
      QMainWindow::keyReleaseEvent(event);//调用基类的事件处理函数
}

//小车运动控制
void carMonitorWin::carRun(QString direction){
    if(direction=="up"){ msg.linear_velocity=1;msg.angular_velocity=0;}
   else if(direction=="down"){ msg.linear_velocity=-1;msg.angular_velocity=0;}
   else if(direction=="left"){ msg.linear_velocity=0;msg.angular_velocity=0.5;}
   else if(direction=="right"){ msg.linear_velocity=0;msg.angular_velocity=-0.5;}
    pub.publish(msg);//发布控制指令 
}
void carMonitorWin::carStop(){
    msg.linear_velocity=0;
    msg.angular_velocity=0;
    pub.publish(msg);
}
//槽函数
void carMonitorWin::button_slot(){
    // 获取发送信号的按钮对象
    QPushButton* clickedButton=qobject_cast<QPushButton*>(sender());
    if(clickedButton==ui->up_pushButton){//前
        qInfo()<<"上被点击了！";
        carRun("up");
    }
    else if(clickedButton==ui->down_pushButton){//后
        qInfo()<<"下被点击了！";
        carRun("down");
    }
    else if(clickedButton==ui->left_pushButton){//左
        qInfo()<<"左被点击了！";
        carRun("left");
    }
    else if(clickedButton==ui->right_pushButton){//右 
        qInfo()<<"右被点击了！";
       carRun("right");
    }
    motionTimer.start(300);//300ms后停止运动
      
}
//小车运动方向显示
void carMonitorWin::direct_slot(QString direct){
    qInfo()<<"小车状态:"<<direct;
    if(direct=="up"){ui->up_pushButton->setStyleSheet("background-color:rgb(115, 210, 22);");}
    else if(direct=="down"){ui->down_pushButton->setStyleSheet("background-color:rgb(115, 210, 22);");}
    else if(direct=="left"){ui->left_pushButton->setStyleSheet("background-color:rgb(115, 210, 22);");}
    else if(direct=="right"){ui->right_pushButton->setStyleSheet("background-color:rgb(115, 210, 22);");}
    // qInfo()<<"背景颜色改变";
    if(preDirect!=direct){
         timer.start(200);//200ms后按键自动恢复背景
    }
   
    preDirect=direct;
}   
//按键按下背景颜色改变
// void carMonitorWin::buttonPressed_Bg_slot(){
//     QPushButton* clickedButton=qobject_cast<QPushButton*>(sender());
//     clickedButton->setStyleSheet("background-color:rgb(115, 210, 22);");
//     qInfo()<<"背景颜色改变";
//     timer.start(300);//500ms后按键自动恢复背景
// }
//按键松开背景颜色改变
void carMonitorWin::buttonReleased_Bg_slot(){
    //QPushButton* clickedButton=qobject_cast<QPushButton*>(sender());
    ui->up_pushButton->setStyleSheet("");
    ui->down_pushButton->setStyleSheet("");
    ui->left_pushButton->setStyleSheet("");
    ui->right_pushButton->setStyleSheet("");
    // qInfo()<<"背景颜色恢复";
}
 void  carMonitorWin::motionTimer_slot(){
        carStop();
 }


carMonitorWin::~carMonitorWin()
{
    delete ui; 
}